#!/usr/bin/env python

import six
import rospy
import rospkg
from interbotix_xs_modules.locobot import InterbotixLocobotXS as Locobot
from interbotix_landmark_modules.landmark import LandmarkCollection

# Update this value with your model
MODEL = "px100"

class NavigationInerface(object):
    landmarks = LandmarkCollection(
        ros_on=True)

    def __init__(self):
        self.active_lm = None
        self.tags = []
        self.locobot = Locobot(
            robot_model="locobot_"+MODEL,
            use_move_base_action=True)
        r = rospkg.RosPack()
        pkg_path = r.get_path("interbotix_xslocobot_landmark_nav")
        self.lm_config_filepath = rospy.get_param(
            "~landmark_config",
            "{}/landmarks/landmarks.yaml".format(pkg_path))
        self._load_landmarks()

    def run(self):
        while not rospy.is_shutdown():
            print("Landmarks available to navigate to")
            for lm in self.set_lms:
                print("\t- ({}): {}".format(lm.id_, lm.label_))
                print("\t\tmounted: {}".format(lm.is_mounted()))
                print("\t\tmounted offset:\t{}".format((lm.get_mounted_offset())))
            print("//======================//")

            while True:
                self.active_lm = int(six.moves.input("Choose landmark: "))
                if self.active_lm in self.landmarks.data.keys():
                    print(
                        "You have chosen to nav to {}.".format(
                            self.landmarks.data[self.active_lm].get_label()))
                    break
                else:
                    print("'%s' is not in the list of landmarks. Choose again."
                        % self.active_lm)
            x, y, yaw = self.landmarks.data[self.active_lm].get_mb_goal()

            result = self.locobot.base.move_to_pose(x, y, yaw, wait=True)
            if result:
                print(
                    "Successfully reached landmark {}".format(
                        self.landmarks.data[self.active_lm].get_label()))
                print("Go to another landmark?")
                print("\t[y/n]")
                more = str(six.moves.input("  >  "))
                if (more != "y") or (more != "yes"):
                    break

    def _load_landmarks(self):
        if self.landmarks.load(self.lm_config_filepath):
            self.set_lms = self.landmarks.get_set_landmarks()
            if len(self.set_lms) == 0:
                print("No landmarks are set. Did your robot see any landmarks?")
                exit()
            print("Loaded landmarks from '" + self.lm_config_filepath  + "'.")
            self.landmarks.pub_markers(self.landmarks.get_set_tags())
            self.landmarks.pub_tfs(self.landmarks.get_set_tags())
        else:
            exit()

if __name__ == "__main__":
    nav = NavigationInerface()
    nav.run()
